//
// Created by wsl on 1/6/23.
//
#include "defect_depth.h"
#include <iostream>
#include <utility>
#include <numeric>
// 生成随机点对索引
std::vector<int> getRandomIndices(int maxIndex, int sampleSize) {
    std::vector<int> indices(maxIndex);
    std::iota(indices.begin(), indices.end(), 0);
    random_shuffle(indices.begin(), indices.end());
    indices.resize(sampleSize);
    return indices;
}

Eigen::Vector4d PlaneFittingRansac(const std::vector<Eigen::Vector3d>& points)
{
    int maxIterations=100;
    double distanceThreshold=10;
    int numPoints = points.size();
    int bestInlierCount = 0;
    Eigen::Vector4d bestPlane;

    for (int i = 0; i < maxIterations; ++i) {
        // 随机选取3个点作为平面点
        auto randomIndices = getRandomIndices(numPoints, 3);
        cv::Point3d p1 = {points[randomIndices[0]][0],points[randomIndices[0]][1],points[randomIndices[0]][2]};
        cv::Point3d p2 = {points[randomIndices[1]][0],points[randomIndices[1]][1],points[randomIndices[1]][2]};
        cv::Point3d p3 = {points[randomIndices[2]][0],points[randomIndices[2]][1],points[randomIndices[2]][2]};

        // 计算平面法向量
        Eigen::Vector3d v1(p2.x - p1.x, p2.y - p1.y, p2.z - p1.z);
        Eigen::Vector3d v2(p3.x - p1.x, p3.y - p1.y, p3.z - p1.z);
        Eigen::Vector3d normal = v1.cross(v2).normalized();
        double d = -normal.dot(Eigen::Vector3d(p1.x, p1.y, p1.z));

        // 统计所有距离小于阈值的点数
        int inlierCount = 0;
        for (int j = 0; j < numPoints; ++j) {
            cv::Point3d p = {points[j][0],points[j][1],points[j][2]};
            double distance = abs(normal.dot(Eigen::Vector3d(p.x, p.y, p.z)) + d) / normal.norm();
            if (distance < distanceThreshold) {
                ++inlierCount;
            }
        }

        // 如果当前内点数比之前最优结果要好，则更新最优平面参数
        if (inlierCount > bestInlierCount) {
            bestInlierCount = inlierCount;
            bestPlane << normal, d;
        }
    }

    return bestPlane;
}

Eigen::Vector4d PlaneFitting(const std::vector<Eigen::Vector3d>& points)
{
//    int n = points.size();
//    Eigen::MatrixXd mleft = Eigen::MatrixXd::Ones(n, 3);//初始化左边项
//    Eigen::VectorXd b = Eigen::VectorXd::Zero(n); //初始化右边项
//    for (int i = 0; i < n; ++i)//遍历点装填两个矩阵
//    {
//        mleft(i, 0) = points[i][0];
//        mleft(i, 1) = points[i][1];
//        b[i] = points[i][2];
//    }
//    Eigen::VectorXd x = mleft.fullPivHouseholderQr().solve(b);//QR分解求解
//    float a0 = x[0]; float a1 = x[1]; float a2 = x[2];//为了便于理解，赋值给临时变量
//    double A = a0; double B = a1; double C = -1; double D = a2;
//    return Eigen::Vector4d(A,B,C,-D);
    int numPoints = points.size();

    Eigen::MatrixXd A(numPoints, 4);
    for (int i = 0; i < numPoints; i++) {
        A(i, 0) = points[i].x();
        A(i, 1) = points[i].y();
        A(i, 2) = points[i].z();
        A(i, 3) = 1;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Vector4d planeParam = svd.matrixV().col(3);
    Eigen::Vector4d plane = planeParam / planeParam.head(3).norm();
    return plane;
}



cv::Mat rot_img(cv::Mat img,float angle)
{
    int w=img.cols,h=img.rows;
    int cX=w/2,cY=h/2;
    cv::Mat M=cv::getRotationMatrix2D(cv::Point(cX,cY),-1*angle,1.0);
    double cos=abs(M.at<double>(0,0));
    double sin=abs(M.at<double>(0,1));
    int nW=int(h*sin+w*cos);
    int nH=int(h*cos+w*sin);
    M.at<double>(0,2)+=nW*1.0/2-cX;
    M.at<double>(1,2)+=nH*1.0/2-cY;
    cv::warpAffine(img,img,M,cv::Size(nW,nH),cv::INTER_LINEAR,0);
    return img;
}

std::vector<cv::Point> crop_poly(std::vector<cv::Point> poly,cv::Point refp)
{
    std::vector<cv::Point> dst{};
    for(auto pp:poly)
    {
        dst.emplace_back(pp.x-refp.x,pp.y-refp.y);
    }
    return dst;
}
std::vector<cv::Point> DefectDepth::shrink_poly(std::vector<cv::Point> polygon, float ratio) {
    ClipperLib::Path poly;
    for (auto & i : polygon) {
        poly.push_back(ClipperLib::IntPoint(i.x, i.y));
    }
    double distance = (1 - ratio * ratio) * ClipperLib::Area(poly) / ClipperLib::Perimeter(poly);
    ClipperLib::ClipperOffset clipperOffset;
    clipperOffset.AddPath(poly, ClipperLib::JoinType::jtRound, ClipperLib::EndType::etClosedPolygon);
    ClipperLib::Paths res_polys;
    res_polys.push_back(poly);
    clipperOffset.Execute(res_polys,-1*distance);

    if(!res_polys.empty())
    {
        polygon.clear();
        for (auto & j : res_polys[0]) {
            polygon.emplace_back(j.X, j.Y);
        }
    }
    return polygon;
}

std::vector<double> DefectDepth::get_rot_diff_depth(cv::Mat depth,  std::vector<cv::Point> poly1,
                                                   std::vector<cv::Point> poly2) {
    cv::Mat mask1=cv::Mat::zeros(depth.size(),CV_64FC1);
    cv::Mat mask2=cv::Mat::zeros(depth.size(),CV_64FC1);
    std::vector<std::vector<cv::Point>> pp1{};
    pp1.push_back(poly1);
    std::vector<std::vector<cv::Point>> pp2{};
    pp2.push_back(std::move(poly2));
    cv::fillPoly(mask1,pp1,1);
    cv::fillPoly(mask1,pp2,0);
    cv::fillPoly(mask2,pp2,1);


    cv::RotatedRect rot_rect=cv::minAreaRect(poly1);
    depth= rot_img(depth,-1*rot_rect.angle);
    mask1= rot_img(mask1,-1*rot_rect.angle);
    mask2= rot_img(mask2,-1*rot_rect.angle);
    if(this->viz)
    {
        cv::imshow("a",mask1);
        cv::imshow("b",mask2);
        cv::waitKey(0);
    }

    int h=depth.rows,w=depth.cols;
    std::vector<double> diff_depth{};
    if(h>w)
    {
        for(int i=0;i< this->items;i++)
        {
            int y0=int(i*h/ this->items);
            int y1=y0+ h/ this->items;
            cv::Mat part_depth=depth(cv::Rect(0,y0,w,y1-y0));
            cv::Mat part_mask1=mask1(cv::Rect(0,y0,w,y1-y0));
            cv::Mat part_mask2=mask2(cv::Rect(0,y0,w,y1-y0));
            auto d1=cv::sum(part_depth.mul(part_mask1))/(cv::sum(part_mask1)[0]+1e-7);
            auto d2=cv::sum(part_depth.mul(part_mask2))/(cv::sum(part_mask2)[0]+1e-7);
            double dd=d1[0]-d2[0];
            if(cv::sum(part_mask2)[0]>0)
            {
                diff_depth.emplace_back(dd);
            } else
            {
                diff_depth.emplace_back(0);
            }

        }
    } else
    {
        for(int i=0;i< this->items;i++)
        {
            int x0=int(i*w/ this->items);
            int x1=x0+ w/ this->items;
            cv::Mat part_depth=depth(cv::Rect(x0,0,x1-x0,h));
            cv::Mat part_mask1=mask1(cv::Rect(x0,0,x1-x0,h));
            cv::Mat part_mask2=mask2(cv::Rect(x0,0,x1-x0,h));
            auto d1=cv::sum(part_depth.mul(part_mask1))/(cv::sum(part_mask1)[0]+1e-7);
            auto d2=cv::sum(part_depth.mul(part_mask2))/(cv::sum(part_mask2)[0]+1e-7);
            double dd=d1[0]-d2[0];
            if(cv::sum(part_mask2)[0]>0)
            {
                diff_depth.emplace_back(dd);
            } else
            {
                diff_depth.emplace_back(0);
            }
        }
    }
    return diff_depth;

}

double DefectDepth::get_plane_diff_depth(cv::Mat depth, std::vector<cv::Point> poly1, std::vector<cv::Point> poly2) {
    cv::Mat mask1=cv::Mat::zeros(depth.size(),CV_64FC1);
    cv::Mat mask2=cv::Mat::zeros(depth.size(),CV_64FC1);
    std::vector<std::vector<cv::Point>> pp1{};
    pp1.push_back(poly1);
    std::vector<std::vector<cv::Point>> pp2{};
    pp2.push_back(std::move(poly2));
    cv::fillPoly(mask1,pp1,1);
    cv::fillPoly(mask1,pp2,0);
    cv::fillPoly(mask2,pp2,1);

    std::vector<Eigen::Vector3d> pts1{};
    std::vector<Eigen::Vector3d> pts2{};
    double xm=0,ym=0,zm=0;
    double defect_xyz[3]{0,0,0};
    double max_depth=-9999999;
    double min_depth=9999999;
    for(int row=0;row<depth.rows;row++)
    {
        double *curr_depth=depth.ptr<double>(row);
        double *curr_mask1=mask1.ptr<double>(row);
        double *curr_mask2=mask2.ptr<double>(row);
        for(int col=0;col<depth.cols;col++)
        {
            double z=curr_depth[col];
            if(z==-26214.4)
            {
                continue;
            }
            if(curr_mask1[col]>0)
            {
                pts1.emplace_back(col,row,z);
                if(min_depth>z)
                {
                    min_depth=z;
                }
            }
            if(curr_mask2[col]>0)
            {
                pts2.emplace_back(col,row,z);
                xm+=col;
                ym+=row;
                zm+=z;
                if(z>max_depth)
                {
                    max_depth=z;
                    defect_xyz[0]=col;
                    defect_xyz[1]=row;
                    defect_xyz[2]=z;
                }
                if(min_depth>z)
                {
                    min_depth=z;
                }
            }
        }
    }

    xm/=pts2.size();
    ym/=pts2.size();
    zm/=pts2.size();
    auto plan1=PlaneFitting(pts1);
    double diff_height= abs(plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
            +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
//    auto plan2=PlaneFitting(pts2);
//    double z2=(-plan2[3]-plan2[0]*xm-plan2[1]*ym)/plan2[2];
//    double z1=(-plan1[3]-plan1[0]*xm-plan1[1]*ym)/plan1[2];
//    //double dis= abs(plan1[0]*xm+plan1[1]*ym+plan1[2]*zm+plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
//    std::cout<<plan1[0]<<" "<<plan1[1]<<" "<<plan1[2]<<" "<<plan1[3]<<" "<<std::endl;
//    std::cout<<plan2[0]<<" "<<plan2[1]<<" "<<plan2[2]<<" "<<plan2[3]<<" "<<std::endl;
    std::cout<<diff_height<<std::endl;
    //std::cout<<max_depth-min_depth<<std::endl;
    return diff_height;
}

std::vector<double>
DefectDepth::get_polys_depth(cv::Mat depth, const cv::Mat& gray, std::vector<std::vector<cv::Point>> points) {
    depth.convertTo(depth,CV_64F);
    depth=(depth-32768) * 0.8;

    auto model=DefectDepth();
    std::vector<double> defect_value{};
    for(auto poly:points)
    {
        auto poly1=model.shrink_poly(poly,1.2);

        auto rect0=cv::minAreaRect(poly);
        auto rect1=cv::minAreaRect(poly1);
        if((rect0.size.width*rect0.size.height)>(rect1.size.width*rect1.size.height))
        {
            poly1=model.shrink_poly(poly,0.8);
        }
        rect1=cv::minAreaRect(poly1);
        assert((rect0.size.width*rect0.size.height)<(rect1.size.width*rect1.size.height));
        int x0=1e7,y0=1e7,x1=0,y1=0;
        for(const auto& pp:poly1)
        {
            x0=cv::min(x0,pp.x);
            y0=cv::min(y0,pp.y);
            x1=cv::max(x1,pp.x);
            y1=cv::max(y1,pp.y);
        }
        auto window_depth=depth(cv::Rect(x0,y0,x1-x0,y1-y0));
        auto window_gray=gray(cv::Rect(x0,y0,x1-x0,y1-y0));
        for(auto& pp:poly1)
        {
            pp.x-=x0;
            pp.y-=y0;
        }
        for(auto& pp:poly)
        {
            pp.x-=x0;
            pp.y-=y0;
        }
        //auto res=model.get_plane_diff_depth(window_depth,poly1,poly);

        auto res=model.get_rot_diff_depth(window_depth,poly1,poly);
        double max_depth=0;
        for(auto& pp:res)
        {
            max_depth= std::max(max_depth, abs(pp));
        }
        defect_value.push_back(max_depth);
        if(this->viz)
        {
            std::cout<<max_depth<<std::endl;
            cv::polylines(window_gray,{poly1}, true,cv::Scalar(255,0,0));
            cv::polylines(window_gray,{poly}, true,cv::Scalar(0,0,255));
            cv::imshow("a",window_gray);
            cv::waitKey(0);
        }

    }
    return defect_value;
}

std::vector<double>
DefectDepth::get_depth_basemask(cv::Mat depth, cv::Mat detect_mask, const std::vector<std::vector<cv::Point>>& polys,const cv::Mat& M) {
    std::vector<cv::Mat> channels{};
    cv::split(detect_mask.clone(),channels);
    detect_mask=channels[1]-channels[2];
    //cv::warpPerspective(detect_mask,detect_mask,M,depth.size());

    cv::Mat mask=(depth!=0);
    mask.convertTo(mask,CV_64F);
    depth.convertTo(depth,CV_64F);
    detect_mask.convertTo(detect_mask,CV_64F);
    depth=(depth-32768) * 1.6;

    std::vector<Eigen::Vector3d> pts1{};

    for(int row=0;row<depth.rows;row++)
    {
        double *curr_depth=depth.ptr<double>(row);
        double *curr_mask1=detect_mask.ptr<double>(row);
        double *curr_mask2=mask.ptr<double>(row);
        for(int col=0;col<depth.cols;col++)
        {
            double z=curr_depth[col];
            if(curr_mask1[col]>0 &&(curr_mask2[col]>0))
            {
                pts1.emplace_back(col,row,z);

            }
        }
    }
    auto plan1=PlaneFitting(pts1);
    std::cout<<"plane:"<<plan1[0]<<"*x+"<<plan1[1]<<"*y+"<<plan1[2]<<"*z+"<<plan1[3]<<std::endl;


    std::vector<double> defect_value{};
    for(const auto& pp:polys)
    {
        auto poly=pp;
        //auto poly= warpPoly(pp,M);
        cv::RotatedRect rot_rect=cv::minAreaRect(poly);
        cv::Rect roi=rot_rect.boundingRect();
        cv::Mat window_depth=depth(roi);
        cv::Mat window_mask=mask(roi);


        if(cv::sum(window_mask)[0]<1)
        {
            defect_value.push_back(-1);
            continue;
        }
        double defect_xyz[3]{0,0,0};
        double max_depth=-9999999;
        for(int row=0;row<window_depth.rows;row++)
        {
            double *curr_depth=window_depth.ptr<double>(row);
            double *curr_mask2=window_mask.ptr<double>(row);
            for(int col=0;col<window_depth.cols;col++)
            {
                double z=curr_depth[col];
                if(curr_mask2[col]>0)
                {
                    if(z>max_depth)
                    {
                        defect_xyz[0]=col+roi.x;
                        defect_xyz[1]=row+roi.y;
                        defect_xyz[2]=z;
                        max_depth=z;
                    }

                }
            }
        }
        double diff_height= abs(plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
                                +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
        defect_value.push_back(diff_height);
    }
    return defect_value;

}

std::vector<double>
DefectDepth::get_depth_minmax(cv::Mat depth, std::vector<std::vector<cv::Point>> polys, const cv::Mat &M) {
    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * 1.6;
    std::vector<double> defect_values{};
    for (auto pp: polys) {
        auto poly = warpPoly(pp, M);
        //poly = shrink_poly(poly, 4);
//        auto rect0 = cv::minAreaRect(poly);
//        auto rect1 = cv::minAreaRect(poly1);
//        if ((rect0.size.width * rect0.size.height) > (rect1.size.width * rect1.size.height)) {
//            poly1 = shrink_poly(poly, 0.8);
//        }
//        rect1 = cv::minAreaRect(poly1);
//        assert((rect0.size.width * rect0.size.height) < (rect1.size.width * rect1.size.height));
        auto rect1 = cv::minAreaRect(poly);
        auto roi = rect1.boundingRect();
        auto window_depth = depth(roi);
        auto window_mask = mask(roi);
        double min_depth = 999999;
        double max_depth = -999999;
        for (int row = 0; row < window_depth.rows; row++) {
            double *curr_depth = window_depth.ptr<double>(row);
            double *curr_mask1 = window_mask.ptr<double>(row);
            for (int col = 0; col < window_depth.cols; col++) {
                double z = curr_depth[col];

                if (curr_mask1[col] <255) {
                    continue;
                }
                if (min_depth > z) {
                    min_depth = z;
                }
                if (z > max_depth) {
                    max_depth = z;
                }
            }
        }
        //std::cout<<max_depth<<"   "<<min_depth<<std::endl;
        defect_values.push_back(max_depth - min_depth);
    }
    return defect_values;
}

double DefectDepth::test_line_plane_depth(cv::Mat depth,cv::Mat& gray, std::vector<cv::Point> poly, Circle c0) {
    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * 1.6;

    auto rect1 = cv::minAreaRect(poly);
    auto roi = rect1.boundingRect();
    auto window_depth = depth(roi);
    auto window_mask = mask(roi);
    double min_depth = 999999;
    double max_depth = -999999;
    int xy[2]={0,0};
    cv::Point minp;
    for (int row = 0; row < window_depth.rows; row++) {
        double *curr_depth = window_depth.ptr<double>(row);
        double *curr_mask1 = window_mask.ptr<double>(row);
        for (int col = 0; col < window_depth.cols; col++) {
            double z = curr_depth[col];

            if (curr_mask1[col] <255) {
                continue;
            }
            if (z > max_depth) {
                max_depth = z;
                xy[0]=col+roi.x;
                xy[1]=row+roi.y;
            }
//            if (z < min_depth) {
//                min_depth = z;
//                minp=cv::Point {col+roi.x,row+roi.y};
//            }
        }
    }
   // std::cout<<xy[0]<<","<<xy[1]<<std::endl;
    std::vector<Eigen::Vector3d> pts1{};
    auto points= getPointsOnLine(c0,cv::Point(xy[0],xy[1]));
    cv::circle(gray,cv::Point(xy[0],xy[1]),5,cv::Scalar(0,0,255),2);
    double mean_height=0;
    double num=0;
    for(auto pp:points)
    {
        if(mask.at<double>(pp.y,pp.x)<255)
            continue;
        cv::circle(gray,pp,2,cv::Scalar(255,0,0),1);
        double z=depth.at<double>(pp.y,pp.x);
        //std::cout<<pp.x<<" "<<pp.y<<" "<<z<<std::endl;
        pts1.emplace_back(pp.x,pp.y,z);
        mean_height+=z;
        num+=1;
        if(min_depth>z)
        {
            min_depth=z;
            minp=pp;
        }
    }
    auto plan1=PlaneFitting(pts1);
//    for(auto defect_xyz:pts1)
//    {
//        double zz=plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
//                  +plan1[3];
//        std::cout<<zz<<std::endl;
//    }
    std::cout<<"plane:"<<plan1[0]<<"*x+"<<plan1[1]<<"*y+"<<plan1[2]<<"*z+"<<plan1[3]<<std::endl;
    double defect_xyz[3]={double(xy[0]),double(xy[1]),max_depth};
    double diff_height= abs(plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
                            +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
    cv::circle(gray,minp,5,cv::Scalar(255,0,255),2);
    //min_depth=depth.at<double>(points[0].y,points[0].x);
    std::cout<<max_depth<<" "<<min_depth<<" " <<max_depth-min_depth<<" plane height:"<<diff_height<<" "<<max_depth-mean_height/num<<std::endl;


}

std::vector<double>
DefectDepth::get_line_plane_depth(cv::Mat depth, cv::Mat &gray, std::vector<std::vector<cv::Point>> polys, Circle c0,
                                  const cv::Mat &M) {
    std::vector<double> defect_res{};
//    int kernel_size = 5;
//    medianBlur(depth,depth, kernel_size);
    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * this->alpha;

    cv::circle(gray,c0.center, int(this->edage_r),cv::Scalar(0,0,255),2);
    cv::circle(gray,c0.center, int(this->small_r),cv::Scalar(0,0,255),2);
    cv::circle(gray,c0.center, 3,cv::Scalar(0,0,255),2);
    //cv::circle(gray,c0.center, int(c0.radius),cv::Scalar(255,0,0),2);
    for(auto pp:polys)
    {
        auto poly=pp;
        //auto poly= warpPoly(pp,M);
        cv::polylines(gray,{poly},true,cv::Scalar(255,255,0),3);
        auto rect1 = cv::minAreaRect(poly);
        auto roi = rect1.boundingRect();
        auto window_depth = depth(roi);

//        int kernel_size = 5;
//        medianBlur(window_depth,window_depth, kernel_size);

        auto window_mask = mask(roi);
        double min_depth = 999999;
        double max_depth = -999999;
        cv::Point p_max{0,0};
        cv::Point p_min{0,0};
        for (int row = 0; row < window_depth.rows; row++) {
            double *curr_depth = window_depth.ptr<double>(row);
            double *curr_mask1 = window_mask.ptr<double>(row);
            for (int col = 0; col < window_depth.cols; col++) {
                double z = curr_depth[col];

                if (curr_mask1[col] <255) {
                    continue;
                }
                double dx=col+roi.x-c0.center.x;
                double dy=row+roi.y-c0.center.y;
                double distance_circle= sqrt(dx*dx+dy*dy);
                if(distance_circle> this->edage_r)
                {
                    continue;
                }
                if (z > max_depth) {
                    max_depth = z;
                    p_max={col+roi.x,row+roi.y};
                }
//            if (z < min_depth) {
//                min_depth = z;
//                minp=cv::Point {col+roi.x,row+roi.y};
//            }
            }
        }
        if(p_max.x==0 && p_max.y==0)
        {
            defect_res.emplace_back(-1);
            continue;
        }
        int x0=p_max.x-10,y0=p_max.y-10;
//        for(int i=0;i<20;i++)
//        {
//            double *curr_depth = depth.ptr<double>(i+y0);
//            for(int j=0;j<20;j++)
//            {
//                std::cout<<curr_depth[j+x0]<<" ";
//            }
//            std::cout<<std::endl;
//        }

        std::vector<Eigen::Vector3d> pts1{};
        auto points= getPointsOnLine(c0,p_max);
        cv::circle(gray,p_max,5,cv::Scalar(0,0,255),2);
        double mean_height=0;
        double num=0;
//        int y0=99999,y1=-99999;
//        int x0=99999,x1=-99999;
//        for(auto pp:points)
//        {
//            y0= std::min(y0,pp.y);
//            y1=std::max(y1,pp.y);
//
//            x0=std::min(x0,pp.x);
//            x1=std::max(x1,pp.x);
//        }
//        y0-=10;
//        y1+=10;
//        int window_r=10;
//       // std::cout<<x0<<" "<<x1<<" "<<y0<<" "<<y1<<std::endl;
//        for(int x=x0;x<x1;x++)
//        {
//            for(int y=y0;y<y1;y++)
//            {
//                if(mask.at<double>(y,x)<255)
//                    continue;
//                if(mask.at<double>(y-window_r,x)<255||(mask.at<double>(y+window_r,x)<255)||
//                        (mask.at<double>(y,x+window_r))<255
//                        ||(mask.at<double>(y,x+window_r)<255))
//                    continue;
//                double dx=x-c0.center.x;
//                double dy=y-c0.center.y;
//                double distance_circle= sqrt(dx*dx+dy*dy);
//                if(distance_circle<small_r)
//                {
//                    continue;
//                }
//                cv::circle(gray,cv::Point {x,y},2,cv::Scalar(255,255,0),1);
//                double z=depth.at<double>(y,x);
//                //std::cout<<pp.x<<" "<<pp.y<<" "<<z<<std::endl;
//                pts1.emplace_back(x,y,z);
//            }
//        }
//        if(pts1.size()<6)
//        {
//            defect_res.push_back(-1);
//            continue;
//        }
//        auto plan1=PlaneFitting(pts1);
//        std::cout<<"plane:"<<plan1[0]<<"*x+"<<plan1[1]<<"*y+"<<plan1[2]<<"*z+"<<plan1[3]<<std::endl;
//
//        if(cv::sum(window_mask)[0]<1)
//        {
//            defect_res.push_back(-1);
//            continue;
//        }
//            double defect_xyz[3]{0,0,0};
//            max_depth=-9999999;
//            for(int row=0;row<window_depth.rows;row++)
//            {
//                double *curr_depth=window_depth.ptr<double>(row);
//                double *curr_mask2=window_mask.ptr<double>(row);
//                for(int col=0;col<window_depth.cols;col++)
//                {
//                    double z=curr_depth[col];
//                    if(curr_mask2[col]>0)
//                    {
//
//                        defect_xyz[0]=col+roi.x;
//                        defect_xyz[1]=row+roi.y;
//                        defect_xyz[2]=z;
//
//
//                        double diff_height= abs(plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
//                                                +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
////                        if(diff_height>500)
////                        {
////                            std::cout<<diff_height<<std::endl;
////                        }
//
//                        if(diff_height>max_depth)
//                        {
//                            max_depth=diff_height;
//                            p_max={col+roi.x,row+roi.y};
//                        }
//                    }
//
//                }
//            }
//        defect_res.push_back(max_depth);
//        cv::circle(gray,p_max,5,cv::Scalar(255,0,255),2);
//
//    }

        for(auto pp:points)
        {
            if(mask.at<double>(pp.y,pp.x)<255)
                continue;
            double dx=pp.x-c0.center.x;
            double dy=pp.y-c0.center.y;
            double distance_circle= sqrt(dx*dx+dy*dy);
            if(distance_circle<small_r)
            {
                continue;
            }
            cv::circle(gray,pp,2,cv::Scalar(255,255,0),1);
            double z=depth.at<double>(pp.y,pp.x);
            //std::cout<<pp.x<<" "<<pp.y<<" "<<z<<std::endl;
            pts1.emplace_back(pp.x,pp.y,z);
            mean_height+=z;
            num+=1;
            if(min_depth>z)
            {
                min_depth=z;
                p_min=pp;
            }
        }
        if(p_min.x==0&&p_min.y==0)
        {
            defect_res.emplace_back(-1);
            continue;
        }
//        auto plan1=PlaneFitting(pts1);
//        std::cout<<"plane:"<<plan1[0]<<"*x+"<<plan1[1]<<"*y+"<<plan1[2]<<"*z+"<<plan1[3]<<std::endl;
//        double defect_xyz[3]={double(p_max.x),double(p_max.y),max_depth};
//        double diff_height= abs(plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
//                                +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
        cv::circle(gray,p_min,5,cv::Scalar(255,0,255),2);
        //min_depth=depth.at<double>(points[0].y,points[0].x);
        //std::cout<<max_depth<<" "<<mean_height/num<<diff_height<<std::endl;
        double dx=p_max.x-c0.center.x;
        double dy=p_max.y-c0.center.y;
        double distance_circle= sqrt(dx*dx+dy*dy);
        if(distance_circle< this->edage_r)
        {
            defect_res.emplace_back(-2);
            continue;
        }
        if(this->mean_flag)
        {
            defect_res.emplace_back(max_depth-mean_height/num);
        } else
        {
            defect_res.emplace_back(max_depth-min_depth);
        }

    }
    return defect_res;
}

std::vector<double>
DefectDepth::get_line_planev2_depth(cv::Mat depth, cv::Mat &gray, std::vector<std::vector<cv::Point>> polys, Circle c0,
                                  const cv::Mat &M) {
    std::vector<double> defect_res{};
    int kernel_size = 5;
    medianBlur(depth,depth, kernel_size);
    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * this->alpha;

    cv::circle(gray,c0.center, int(this->edage_r),cv::Scalar(0,0,255),2);
    cv::circle(gray,c0.center, int(this->small_r),cv::Scalar(0,0,255),2);
    cv::circle(gray,c0.center, 3,cv::Scalar(0,0,255),2);
    //cv::circle(gray,c0.center, int(c0.radius),cv::Scalar(255,0,0),2);
    for(auto pp:polys)
    {
        auto poly=pp;
        //auto poly= warpPoly(pp,M);
        cv::polylines(gray,{poly},true,cv::Scalar(255,255,0),3);
        auto rect1 = cv::minAreaRect(poly);
        auto roi = rect1.boundingRect();
        auto window_depth = depth(roi);

        auto window_mask = mask(roi);

        cv::Mat window_poly_mask=cv::Mat::zeros(cv::Size (roi.width,roi.height),CV_8UC1);
        auto croped_poly= crop_poly(poly,{roi.x,roi.y});
        std::vector<std::vector<cv::Point>> croped_polys{};
        croped_polys.emplace_back(croped_poly);
        cv::fillPoly(window_poly_mask,croped_polys,cv::Scalar(255));
//        cv::imshow("a",window_poly_mask);
//        cv::waitKey(0);
        window_poly_mask.convertTo(window_poly_mask,CV_64F);
        double min_depth = 999999;
        double max_depth = -999999;
        cv::Point p_max{0,0};
        cv::Point p_min{0,0};
        for (int row = 0; row < window_depth.rows; row++) {
            double *curr_depth = window_depth.ptr<double>(row);
            double *curr_mask1 = window_mask.ptr<double>(row);
            double *curr_mask2 = window_poly_mask.ptr<double>(row);
            for (int col = 0; col < window_depth.cols; col++) {
                double z = curr_depth[col];

                if (curr_mask1[col] <255||curr_mask2[col]<255) {
                    continue;
                }
                double dx=col+roi.x-c0.center.x;
                double dy=row+roi.y-c0.center.y;
                double distance_circle= sqrt(dx*dx+dy*dy);
                if(distance_circle> this->edage_r)
                {
                    continue;
                }
                if (z > max_depth) {
                    max_depth = z;
                    p_max={col+roi.x,row+roi.y};
                }
            }
        }
        if(p_max.x==0 && p_max.y==0)
        {
            defect_res.emplace_back(-1);
            continue;
        }

        std::vector<Eigen::Vector3d> pts1{};
        double mean_height=0;
        double num=0;

        int y0=99999,y1=-99999;
        int x0=99999,x1=-99999;
        for(auto pp:poly)
        {
            y0= std::min(y0,pp.y);
            y1=std::max(y1,pp.y);

            x0=std::min(x0,pp.x);
            x1=std::max(x1,pp.x);
        }
        int window_r=10;
        auto cmax=c0;cmax.radius= this->mid_r;
        auto cmin=c0;cmin.radius= this->small_r;
        auto corners=getPolysOnCircle(cmax,cmin,{x0,y0},{x1,y1});
        cv::polylines(gray,{corners}, true,cv::Scalar(255,0,0),3);
        auto rect_corner=cv::boundingRect(corners);
        auto cur_depth=depth(rect_corner);
        auto cur_mask=mask(rect_corner);
        cv::Mat corner_mask=cv::Mat::zeros(cv::Size(rect_corner.width,rect_corner.height),CV_8UC1);
        std::vector<cv::Point> ref_corner{};
        for(auto pp:corners)
        {
            ref_corner.emplace_back(pp.x-rect_corner.x,pp.y-rect_corner.y);
        }
        std::vector<std::vector<cv::Point>> temp{};
        temp.push_back(ref_corner);
        cv::fillPoly(corner_mask,temp,cv::Scalar(255));
        corner_mask.convertTo(corner_mask, CV_64F);
        for (int row = 0; row < cur_depth.rows; row++) {
            double *curr_depth = cur_depth.ptr<double>(row);
            double *curr_mask1 = cur_mask.ptr<double>(row);
            double *curr_mask2 = corner_mask.ptr<double>(row);
            for (int col = 0; col < cur_depth.cols; col++)
            {
                double z = curr_depth[col];

                if (curr_mask1[col] <255||curr_mask2[col]<255) {
                    continue;
                }
                int y=row+rect_corner.y;int x=col+rect_corner.x;
                if(mask.at<double>(y-window_r,x)<255||(mask.at<double>(y+window_r,x)<255)||
                   (mask.at<double>(y,x+window_r))<255
                   ||(mask.at<double>(y,x+window_r)<255))
                    continue;
                cv::circle(gray,cv::Point {x,y},2,cv::Scalar(255,255,0),1);
                double dx = x - c0.center.x;
                double dy = y - c0.center.y;
                double distance_circle = sqrt(dx * dx + dy * dy);
                if (distance_circle < small_r) {
                    continue;
                }

                mean_height += z;
                num += 1;
                pts1.emplace_back(x, y, z);
                //cv::circle(gray,cv::Point {x,y},2,cv::Scalar(255,255,0),1);
            }
        }

        if(pts1.size()<6)
        {
            defect_res.push_back(-1);
            continue;
        }
        //std::cout<<max_depth-mean_height/num<<std::endl;
        //auto plan1=PlaneFitting(pts1);
        auto plan1=PlaneFittingRansac(pts1);
        std::cout<<"plane:"<<plan1[0]<<"*x+"<<plan1[1]<<"*y+"<<plan1[2]<<"*z+"<<plan1[3]<<std::endl;
        if(cv::sum(window_mask)[0]<1)
        {
            defect_res.push_back(-1);
            continue;
        }
        double defect_xyz[3]{0,0,0};
        max_depth=-9999999;
        for(int row=0;row<window_depth.rows;row++)
        {
            double *curr_depth=window_depth.ptr<double>(row);
            double *curr_mask2=window_mask.ptr<double>(row);
            double *curr_mask_poly=window_poly_mask.ptr<double>(row);
            for(int col=0;col<window_depth.cols;col++)
            {
                double z=curr_depth[col];
                if(curr_mask_poly[col]<255)
                {
                    continue;
                }
                if(curr_mask2[col]>0)
                {

                    defect_xyz[0]=col+roi.x;
                    defect_xyz[1]=row+roi.y;
                    defect_xyz[2]=z;


                    double diff_height= (plan1[0]*defect_xyz[0]+plan1[1]*defect_xyz[1]+plan1[2]*defect_xyz[2]
                                            +plan1[3])/ sqrt(plan1[0]*plan1[0]+plan1[1]*plan1[1]+plan1[2]*plan1[2]);
//                    if(abs(diff_height)>600)
//                    std::cout<<diff_height<<std::endl;
                    if(diff_height>max_depth)
                    {
                        max_depth=diff_height;
                        p_max={col+roi.x,row+roi.y};
                    }
                }

            }
        }
        defect_res.push_back(max_depth);
        //defect_res.push_back(depth.at<double>(p_max.y,p_max.x)-mean_height/num);
        cv::circle(gray,p_max,5,cv::Scalar(255,0,255),2);

    }
    return defect_res;
}

std::vector<double>
DefectDepth::get_plane_depth(cv::Mat depth, cv::Mat &gray, std::vector<std::vector<cv::Point>> polys, Circle c0, int r0,
                             int r1, const cv::Mat &M) {
    // 定义中值滤波器的核大小
    int kernel_size = 5;
    medianBlur(depth,depth, kernel_size);
    int x0 = c0.center.x - r1 - 50;
    int y0 = c0.center.y - r1 - 50;
    cv::Mat c0_mask = cv::Mat::zeros(cv::Size(r1 * 2 + 100, r1 * 2 + 100), CV_8UC1);
    cv::circle(c0_mask, cv::Point{r1 + 50, r1 + 50}, r1, cv::Scalar(255), -1);
    cv::circle(c0_mask, cv::Point{r1 + 50, r1 + 50}, r0, cv::Scalar(0), -1);
    std::vector<Eigen::Vector3d> pts1{};

    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * 1.6;

    for (int row = 0; row < c0_mask.rows; row++) {
        double *curr_depth = depth.ptr<double>(row + y0);
        double *curr_mask1 = c0_mask.ptr<double>(row);
        double *curr_mask2 = mask.ptr<double>(row + y0);

        int window_r=20;
        double *mask_t = mask.ptr<double>(row + y0-window_r);
        double *mask_b = mask.ptr<double>(row + y0+window_r);
        for (int col = 0; col < c0_mask.cols; col++) {
            if(mask_t[col+x0]==0||mask_b[col+x0]==0)
            {
                continue;
            }
            double z = curr_depth[col + x0];
            if(curr_mask2[col+x0-window_r]==0||curr_mask2[col+x0+window_r]==0)
            {
                continue;
            }
            if (curr_mask1[col] > 0 && (curr_mask2[col + x0] > 0)) {
                pts1.emplace_back(col+x0, row+y0, z);

            }
        }
    }
    std::cout<<pts1.size()<<std::endl;
    auto plan1 = PlaneFitting(pts1);
    std::vector<double> defect_res{};
    for (auto pp: polys) {
        auto poly = pp;
        std::vector<double> heights{};
        //auto poly= warpPoly(pp,M);
        cv::polylines(gray, {poly}, true, cv::Scalar(255, 255, 0), 3);
        auto rect1 = cv::minAreaRect(poly);
        auto roi = rect1.boundingRect();
        auto window_depth = depth(roi);
        auto window_mask = mask(roi);
        double min_depth = 999999;
        double max_depth = -999999;
        cv::Point p_max{0, 0};
        cv::Point p_min{0, 0};
        for (int row = 0; row < window_depth.rows; row++) {
            double *curr_depth = window_depth.ptr<double>(row);
            double *curr_mask1 = window_mask.ptr<double>(row);
            for (int col = 0; col < window_depth.cols; col++) {
                double z = curr_depth[col];

                if (curr_mask1[col] < 255) {
                    continue;
                }
                double dx = col + roi.x - c0.center.x;
                double dy = row + roi.y - c0.center.y;
                double distance_circle = sqrt(dx * dx + dy * dy);
                if (distance_circle > this->edage_r || distance_circle < small_r) {
                    continue;
                }
                double defect_xyz[3]{static_cast<double>(col + roi.x), static_cast<double>(row + roi.y), z};
                double diff_height = abs(plan1[0] * defect_xyz[0] + plan1[1] * defect_xyz[1] + plan1[2] * defect_xyz[2]
                                         + plan1[3]) /
                                     sqrt(plan1[0] * plan1[0] + plan1[1] * plan1[1] + plan1[2] * plan1[2]);
//                if(diff_height>500)
//                std::cout<<defect_xyz[0]<<" "<<defect_xyz[1]<<" "<<defect_xyz[2]<<" "<<diff_height<<std::endl;
                if (diff_height > max_depth) {
                    max_depth = diff_height;
                    p_max = {col + roi.x, row + roi.y};
                }
                heights.push_back(diff_height);
//            if (z < min_depth) {
//                min_depth = z;
//                minp=cv::Point {col+roi.x,row+roi.y};
//            }
            }
        }
//        std::sort(heights.begin(), heights.end(), [](int a, int b){return a > b;});
//        for(int i=0;i<30;i++)
//        {
//            std::cout<<heights[i]<<std::endl;
//        }
        //defect_res.push_back((heights[0]+heights[1]+heights[2]+heights[3]+heights[4])/5);
        defect_res.push_back(max_depth);
        cv::circle(gray, p_max, 5, cv::Scalar(0,0,255), -1);

    }
    return defect_res;
}

double simple_height(const cv::Mat& depth,const cv::Mat &mask,const cv::Rect&rect1,const cv::Rect&rect2)
{
    cv::Mat cur_mask1=mask(rect1);
    cv::Mat cur_depth1=depth(rect1);
    cv::Mat cur_mask2=mask(rect2);
    cv::Mat cur_depth2=depth(rect2);
    if(cv::sum(cur_mask1)[0]/(rect1.width*rect1.height)<0.5)
    {
        return -1.0;
    }
    if(cv::sum(cur_mask2)[0]/(rect2.width*rect2.height)<0.5)
    {
        return -1.0;
    }
    double dp1=cv::sum(cur_depth1.mul(cur_mask1))[0]/cv::sum(cur_mask1)[0];
    double dp2=cv::sum(cur_depth2.mul(cur_mask2))[0]/cv::sum(cur_mask2)[0];
    return (dp2-dp1)/1000.;
}
double DefectDepth::get_hangao(cv::Mat depth, cv::Mat &gray, cv::Point c0) {
    cv::Mat mask = (depth != 0);
    mask.convertTo(mask, CV_64F);
    depth.convertTo(depth, CV_64F);
    depth = (depth - 32768) * this->alpha;

    int r1=450;int r2=640;
    cv::circle(gray,c0,r1,cv::Scalar(255,0,0),3);
    cv::circle(gray,c0,r2,cv::Scalar(255,0,0),3);
    int width2=100;double ratio=0.7;
    int distance2=int((r2-r1)*ratio);
    int distance1=370;
    int yt=c0.y-r1-distance2;
    int xt=c0.x-int(width2/2);
    int yt1=c0.y-distance1;
    int xt1=c0.x-int(width2/2);
    double dpt= simple_height(depth,mask,cv::Rect(xt1,yt1,width2,width2),cv::Rect(xt,yt,width2,width2));
    cv::rectangle(gray,cv::Rect(xt,yt,width2,width2),cv::Scalar(255,0,0),3);
    cv::rectangle(gray,cv::Rect(xt1,yt1,width2,width2),cv::Scalar(255,0,0),3);

    int yb=c0.y+r1+distance2-width2;
    int xb=c0.x-int(width2/2);
    int yb1=c0.y+distance1-width2;
    int xb1=c0.x-int(width2/2);
    double dpb= simple_height(depth,mask,cv::Rect(xb1,yb1,width2,width2),cv::Rect(xb,yb,width2,width2));
    cv::rectangle(gray,cv::Rect(xb,yb,width2,width2),cv::Scalar(255,0,0),3);
    cv::rectangle(gray,cv::Rect(xb1,yb1,width2,width2),cv::Scalar(255,0,0),3);

    int yl=c0.y-int(width2/2);
    int xl=c0.x-r1-distance2;
    int yl1=c0.y-int(width2/2);
    int xl1=c0.x-distance1;
    double dpl= simple_height(depth,mask,cv::Rect(xl1,yl1,width2,width2),cv::Rect(xl,yl,width2,width2));
    cv::rectangle(gray,cv::Rect(xl,yl,width2,width2),cv::Scalar(255,0,0),3);
    cv::rectangle(gray,cv::Rect(xl1,yl1,width2,width2),cv::Scalar(255,0,0),3);

    int yr=c0.y-int(width2/2);
    int xr=c0.x+r1+distance2-width2;
    int yr1=c0.y-int(width2/2);
    int xr1=c0.x+distance1-width2;
    double dpr= simple_height(depth,mask,cv::Rect(xr1,yr1,width2,width2),cv::Rect(xr,yr,width2,width2));
    cv::rectangle(gray,cv::Rect(xr,yr,width2,width2),cv::Scalar(255,0,0),3);
    cv::rectangle(gray,cv::Rect(xr1,yr1,width2,width2),cv::Scalar(255,0,0),3);
    std::cout<<dpt<<" "<<dpb<<" "<<dpl<<" "<<dpr<<std::endl;

    return std::max({dpt,dpb,dpl,dpr});
}

cv::Mat warpImage(cv::Mat src,const cv::Mat& M,cv::Mat dst)
{
    cv::warpPerspective(src,src,M,dst.size());
    cv::addWeighted(src,0.6,dst,0.4,0,dst);
    return dst;
}

std::vector<cv::Point> warpPoly( std::vector<cv::Point> poly,const cv::Mat& M)
{
    std::vector<cv::Point> dst{};
    std::vector<cv::Point2f> src{};
    for(const auto& pp:poly)
    {
        src.emplace_back(pp.x,pp.y);
    }
    cv::perspectiveTransform(src,src,M);
    for(const auto& pp:src)
    {
        dst.emplace_back(int(pp.x),int(pp.y));
    }
    return dst;
}
cv::Mat modifyMatrix(std::vector<cv::Point> dst_circles,const std::vector<cv::Point>&  src_circles, int src_camera,cv::Mat M) {
    assert(dst_circles.size()==2);
    assert(src_circles.size()==1);

    auto new_cc= warpPoly(src_circles,M);
    cv::Mat t_m_gray=cv::Mat::eye(cv::Size(3,3),CV_64F);
    int idx=0;
    if(src_camera==4)
        idx=1;
    t_m_gray.at<double>(0,2)=-(new_cc[0].x-dst_circles[idx].x);
    t_m_gray.at<double>(1,2)=-(new_cc[0].y-dst_circles[idx].y);
    M=t_m_gray*M;
    return M;
}